#include "pose_controller/pose_controller.hpp"

#include <cmath>  // For std::fabs, std::atan2, and std::fmod

double normalizeAngle(double angle) {
    return std::fmod(angle + M_PI, 2 * M_PI) - M_PI;
}

double shortestAngularDistance(double start, double goal) {
    double delta = normalizeAngle(goal - start);
    return delta;
}

PoseController::PoseController() : Node("pose_controller_node")
{
    tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);

    rosparamsread();

    twistPublisher_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
    feedbackPublisher_ = this->create_publisher<std_msgs::msg::Bool>("feedback", 10);
    intSubscriber_ = this->create_subscription<geometry_msgs::msg::Pose2D>("act_cmd", 10, std::bind(&PoseController::intHandler, this, std::placeholders::_1));
    stopSubscriber_ = this->create_subscription<std_msgs::msg::Bool>("stop_node", 10, std::bind(&PoseController::stopHandler, this, std::placeholders::_1));
    act_angleSubscriber_ = this->create_subscription<std_msgs::msg::Float64>("act_angle", 10, std::bind(&PoseController::actAngleHandler, this, std::placeholders::_1));
    
    startFlag = false;
    stateCtr = Default;

    poseFlag = false;
    quatFlag = false;

    integral_ = 0.0;
    previous_error_ = 0.0;

    watchdog_timer_ = this->create_wall_timer(
        std::chrono::seconds(1),
        std::bind(&PoseController::watchdogCallback, this)
    );
    
    last_odom_time_ = this->now();  // 初始化为当前时间
}

void PoseController::readcsv() {
    std::ifstream csv_data("/home/ubuntu/eame_ws/src/pose_controller/config/params.csv", std::ios::in);
    std::string line;

    if (!csv_data.is_open()) {
        std::cout << "Error: opening file fail" << std::endl;
        std::exit(1);
    }

    std::istringstream sin;
    std::vector<std::string> words;
    std::string word;

    std::getline(csv_data, line);
    while (std::getline(csv_data, line)) {
        sin.clear();
        sin.str(line);
        words.clear();
        while (std::getline(sin, word, ',')) {
            words.push_back(word);
        }
        params[words[0]] = std::stod(words[1]);
    }
    csv_data.close();
}

void PoseController::rosparamsread() {
    readcsv();

    control_hz = params["control_hz"];
    dis_threshold = params["dis_threshold"];
    dis_angle = params["dis_angle"];
    kp = params["kp"];
    ki = params["ki"];
    kd = params["kd"];
    Vmax = params["Vmax"];
    Lmax = params["Lmax"];

    RCLCPP_INFO(this->get_logger(), "control_hz = %.2f", control_hz);
    RCLCPP_INFO(this->get_logger(), "dis_threshold = %.2f", dis_threshold);
    RCLCPP_INFO(this->get_logger(), "dis_angle = %.2f", dis_angle);
    RCLCPP_INFO(this->get_logger(), "kp = %.2f", kp);
    RCLCPP_INFO(this->get_logger(), "ki = %.2f", ki);
    RCLCPP_INFO(this->get_logger(), "kd = %.2f", kd);
    RCLCPP_INFO(this->get_logger(), "Vmax = %.2f", Vmax);
    RCLCPP_INFO(this->get_logger(), "Lmax = %.2f", Lmax);
}

EulerAngles PoseController::ToEulerAngles(geometry_msgs::msg::Quaternion orientation) {
    tf2::Quaternion quat;
    tf2::fromMsg(orientation, quat);
    tf2::Matrix3x3 mat(quat);
    double roll, pitch, yaw;
    mat.getRPY(roll, pitch, yaw);
    return {roll, pitch, yaw};
}

void PoseController::intHandler(const geometry_msgs::msg::Pose2D::SharedPtr msg) {
    Targetpose.x = msg->x;
    Targetpose.y = msg->y;
    Targetpose.theta = msg->theta;
    startFlag = true;
    poseFlag = true;
    quatFlag = false;
}

void PoseController::actAngleHandler(const std_msgs::msg::Float64::SharedPtr msg) {
    Targetpose.theta = msg->data * (M_PI / 180.0);
    startFlag = true;
    poseFlag = false;
    quatFlag = true;

    integral_ = 0.0;  // 每次收到新目标时重置积分项
    previous_error_ = 0.0;
}

void PoseController::stopHandler(const std_msgs::msg::Bool::SharedPtr msg) {
    bool stopFlag = msg->data;
    if (stopFlag) {
        stop();
    }
}

void PoseController::stop() {
    geometry_msgs::msg::Twist twist;
    twist.linear.x = 0;
    twist.linear.y = 0;
    twist.angular.z = 0;
    twistPublisher_->publish(twist);
    startFlag = false;
    RCLCPP_INFO(this->get_logger(), "Stop");

    std_msgs::msg::Bool feedback_msg;
    feedback_msg.data = false;
    feedbackPublisher_->publish(feedback_msg);
}

void PoseController::watchdogCallback() {
    // 获取当前时间
    rclcpp::Time current_time = this->now();

    // 检查是否超过1秒未收到odom数据
    if ((current_time - last_odom_time_).seconds() > 1.0) {
        RCLCPP_WARN(this->get_logger(), "No odom data received for over 1 second. Stopping the robot.");
        stop();  // 执行停止操作
    }
}

void PoseController::run() {
    rclcpp::Rate loop_rate(control_hz);
    while (rclcpp::ok()) {
        rclcpp::spin_some(this->get_node_base_interface());
        if (startFlag) {
            geometry_msgs::msg::TransformStamped transformStamped;
            try {
                transformStamped = tfBuffer_->lookupTransform("odom", "base_footprint", rclcpp::Time(0));
                last_odom_time_ = this->now();
            } catch (tf2::TransformException &ex) {
                RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
                continue;
            }

            Currentpose.x = transformStamped.transform.translation.x;
            Currentpose.y = transformStamped.transform.translation.y;

            geometry_msgs::msg::Quaternion orientation = transformStamped.transform.rotation;
            EulerAngles angles = ToEulerAngles(orientation);
            currentyaw = angles.yaw;

            if(poseFlag == true && quatFlag ==false)
            {
                float dx = Targetpose.x - Currentpose.x;
                float dy = Targetpose.y - Currentpose.y;
                float d_dis = sqrt(dx * dx + dy * dy);
                RCLCPP_ERROR(this->get_logger(), "dx:%f, dy:%f", dx,dy);

                float heading_angle = atan2(dy, dx);
                float err_angle = heading_angle - currentyaw;
                RCLCPP_ERROR(this->get_logger(), "head angle:%f, err angle:%f", heading_angle,err_angle);

                if (err_angle > PI) {
                    err_angle = err_angle - 2 * PI;
                } else if (err_angle < -PI) {
                    err_angle = err_angle + 2 * PI;
                }

                geometry_msgs::msg::Twist twist;

                if (d_dis < dis_threshold) 
                {
                    twist.linear.x = 0;
                    twist.angular.z = 0;
                    stateCtr = Default;
                    startFlag = false;
                    poseFlag = false;
                    quatFlag = false;
                    RCLCPP_INFO(this->get_logger(), "Reached target pose.");

                    std_msgs::msg::Bool feedback_msg;
                    feedback_msg.data = true;
                    feedbackPublisher_->publish(feedback_msg);
                } else {
                    // Robot is moving towards the target
                    twist.linear.x = linear_x.pid_control_position(d_dis, Lmax, Vmax, kp, ki, kd);
                    twist.angular.z = angular.pid_control_position(err_angle, 1.5, 0.5, kp, ki, kd);
                }

                twistPublisher_->publish(twist);
            }else if(poseFlag == false && quatFlag == true)
            {
                // 计算角度差异
                double angle_diff = shortestAngularDistance(currentyaw, Targetpose.theta);

                RCLCPP_ERROR(this->get_logger(), "angle_diff:%f", angle_diff);
                    
                // 如果已经到达目标角度，则停止
                if (fabs(angle_diff) < dis_angle)  // 增加阈值到0.05
                {
                    geometry_msgs::msg::Twist stop_cmd;
                    stop_cmd.angular.z = 0;
                    twistPublisher_->publish(stop_cmd);
                    startFlag = false;
                    poseFlag = false;
                    quatFlag = false;
                    RCLCPP_INFO(this->get_logger(), "Reached target quat.");

                    std_msgs::msg::Bool feedback_msg;
                    feedback_msg.data = true;
                    feedbackPublisher_->publish(feedback_msg);
                }else{
                    // 计算和发布速度命令
                    double Kp = 0.1;  // 比例系数
                    double Ki = 0.0;  // 积分系数
                    double Kd = 0.0;  // 微分系数

                    integral_ += angle_diff;  // 积分项
                    double derivative = angle_diff - previous_error_;  // 微分项
                    previous_error_ = angle_diff;  // 更新前一个误差

                    integral_ += angle_diff;  // 积分项
                        
                    // 计算和发布速度命令
                    geometry_msgs::msg::Twist cmd;
                    cmd.angular.z = Kp * angle_diff + Ki * integral_ + Kd * derivative;
                    twistPublisher_->publish(cmd);
                }
            }
        }
        loop_rate.sleep();
    }
}